#include "fxcc/core/graph/common/Transform.h"
#include "fxcc/core/graph/common/Config.h"

using namespace fxcc::graph::common;

void Transform::Decompose(const glm::mat4 &source)
{
	CreateFrom(source);
};

Transform::Transform()
	:m_Position(glm::vec3(0.0f)), m_Scale(glm::vec3(1.0)), m_Rotation(glm::vec3(0))
{
}

glm::vec3 Transform::GetRight() const
{

#ifdef FXCC_USE_LEFT_HAND
	return GetRightLH();
#else
	return GetRightRH();
#endif

}

glm::vec3 Transform::GetRightLH() const
{
	glm::mat4 rMat = GetRotationMatrix();
	return -rMat[0];
}

glm::vec3 Transform::GetRightRH() const
{
	glm::mat4 rMat = GetRotationMatrix();
	return rMat[0];
}

glm::vec3 Transform::GetForward() const
{
	glm::mat4 rMat = GetRotationMatrix();
	return rMat[2];
}

glm::vec3 Transform::GetUp() const
{
	glm::mat4 rMat = GetRotationMatrix();
	return rMat[1];
}

glm::mat4 Transform::GetViewMatrix() const
{
#ifdef FXCC_USE_LEFT_HAND
	return GetViewMatrixLH();
#else
	return GetViewMatriRH();
#endif

}

glm::mat4 fxcc::graph::common::Transform::GetViewMatrixNoTranslation() const
{
	return glm::mat4(glm::mat3(GetViewMatrix()));

}
glm::mat4 fxcc::graph::common::Transform::GetViewMatrixLH() const
{
	auto forward = GetForward();
	return glm::lookAtLH(m_Position, m_Position + forward, GetUp());
}

glm::mat4 fxcc::graph::common::Transform::GetViewMatriRH() const
{
	auto forward = GetForward();
	return glm::lookAtRH(m_Position, m_Position + forward, GetUp());
}


void Transform::CreateFrom(const glm::mat4& local)
{
	glm::vec3 skew;
	glm::vec4 perspective;
	glm::quat rot;

	glm::decompose(local, m_Scale, rot, m_Position, skew, perspective);

	m_Rotation = glm::degrees(glm::eulerAngles(rot));
}


glm::mat4 Transform::GetWorld() const
{
	glm::mat4 model(1.0f);

	model = glm::translate(model, m_Position);
	model = glm::rotate(model, glm::radians(m_Rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(m_Rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(m_Rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
	model = glm::scale(model, m_Scale);

	return model;
}

glm::mat4 Transform::GetPosRotMatrix() const
{
	glm::mat4 model(1.0f);
	model = glm::translate(model, m_Position);
	model = glm::rotate(model, glm::radians(m_Rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(m_Rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(m_Rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
	return model;
}

void Transform::RotateX(float d)
{
	m_Rotation.x += d;
}

void Transform::RotateY(float d)
{
#ifdef FXCC_USE_LEFT_HAND
	m_Rotation.y += d;
#else
	m_Rotation.y -= d;

#endif
}

void Transform::RotateZ(float d)
{
	m_Rotation.z += d;
}

void Transform::SetRotation(const glm::quat &q)
{
	this->m_Rotation = glm::degrees(glm::eulerAngles(q));
}

void fxcc::graph::common::Transform::Fixed()
{
	m_Rotation.y *= -1.f;
	float temp = m_Rotation.x;
	m_Rotation.x = m_Rotation.z;
	m_Rotation.z = temp;
	m_Rotation.x += 180.f;
	m_Rotation.x *= -1;
}
void Transform::Scale(float d)
{
	this->m_Scale *= d;
}

void Transform::Scale(glm::vec3 o, float d)
{
	glm::vec3 delta = o - m_Position;
	float len = glm::length(delta);
	glm::vec3 frd = glm::normalize(delta);

	this->m_Position = o;
	this->m_Scale *= d;
	this->m_Position += delta * d * frd;
}

void Transform::BackAt(glm::vec3 target, glm::vec3 worldUp)
{
	if (target == this->m_Position)
	{
		return;
	}

#ifdef FXCC_USE_LEFT_HAND
	glm::mat4 view = glm::lookAtLH(m_Position, target, worldUp);
#else
	glm::mat4 view = glm::lookAtRH(m_Position, target, worldUp);
#endif

	m_Rotation = GetEulerAnglesFromRotationMatrix(glm::inverse(view));
}

void Transform::LookAt(glm::vec3 target, glm::vec3 worldUp)
{
#ifdef FXCC_USE_LEFT_HAND
	BackAt(target, worldUp);

#else
	BackAt(2.0f * m_Position - target, worldUp);

#endif

}

void fxcc::graph::common::Transform::BackAtLH(glm::vec3 target, glm::vec3 worldUp)
{
	if (target == this->m_Position)
	{
		return;
	};
	glm::mat4 view = glm::lookAtLH(m_Position, target, worldUp);
	m_Rotation = GetEulerAnglesFromRotationMatrix(glm::inverse(view));
}

void fxcc::graph::common::Transform::LookAtLH(glm::vec3 pos, glm::vec3 worldUp)
{

	BackAtLH(2.0f * m_Position - pos, worldUp);
}

glm::mat4 Transform::GetRotationMatrix() const
{
	glm::mat4 model(1.0f);
	model = glm::rotate(model, glm::radians(m_Rotation.z), glm::vec3(0.0f, 0.0f, 1.0f));
	model = glm::rotate(model, glm::radians(m_Rotation.y), glm::vec3(0.0f, 1.0f, 0.0f));
	model = glm::rotate(model, glm::radians(m_Rotation.x), glm::vec3(1.0f, 0.0f, 0.0f));
	return model;
}

void Transform::Move(float dist, glm::vec3 f)
{
	m_Position += dist * glm::normalize(f);
}

void Transform::Translate(float dist)
{
	this->m_Position += this->GetForward() * dist;
}

void Transform::MoveRight(float dist)
{
	this->m_Position += this->GetRight() * dist;
}

glm::vec3 Transform::GetPosXZ() const
{
	glm::vec3 forward = GetForward();

	float f = forward.y;
	float p = m_Position.y;

	if (f == 0.0f)
	{
		return glm::vec3();
	}

	if (p * f < 0.0f)
	{
		float len = -p / f;
		return len * forward + m_Position;
	}
	return glm::vec3(0.f);
}

glm::vec3 Transform::GetPosYZ() const
{
	glm::vec3 forward = GetForward();

	float f = forward.x;
	float p = m_Position.x;

	if (p * f <= 0.0f)
	{
		float len = -p / f;
		return len * forward + m_Position;
	}
	return glm::vec3(0.0f);
}

glm::vec3 Transform::GetPosXY() const
{
	glm::vec3 forward = GetForward();

	float f = forward.z;
	float p = m_Position.z;

	if (p * f <= 0.0f)
	{
		float len = -p / f;
		return len * forward + m_Position;
	}
	return glm::vec3();
}

float Transform::GetToXZSq()
{
	glm::vec3 d2 = m_Position - GetPosYZ();
	return glm::length(d2);
}

void Transform::Rotate(glm::vec3 axis)
{
	RotateZ(axis.z);
	RotateX(axis.y);
	RotateY(axis.z);
}

Transform::Transform(const glm::vec3 &pos, const glm::quat &rot, const glm::vec3 &m_Scale)
{
	this->m_Position = pos;
	SetRotation(rot);
	this->m_Scale = m_Scale;
}

glm::quat Transform::GetRotationQuat() const
{
	return glm::quat(glm::radians(m_Rotation));
}

void Transform::SetPosition(const glm::vec3 &pos)
{
	this->m_Position = pos;
}

void Transform::SetScale(const glm::vec3 &s)
{
	this->m_Scale = s;
}

glm::vec3 Transform::Yup()
{
	return glm::vec3(0.0f, 1.0f, 0.0f);
}
glm::vec3 Transform::Xup()
{
	return glm::vec3(1.0f, .0f, 0.0f);
}

glm::vec3 Transform::Zup()
{
	return glm::vec3(0.0f, .0f, 1.0f);
}

Transform Transform::Slerp(const Transform &t1, const Transform &t2, float factor)
{
	Transform result;
	result.m_Position = glm::mix(t1.m_Position, t2.m_Position, factor);
	//result.m_Rotation = glm::mix(t1.m_Rotation, t2.m_Rotation, factor);
	result.m_Scale = glm::mix(t1.m_Scale, t2.m_Scale, factor);

	glm::quat q1 = t1.GetRotationQuat();
	glm::quat q2 = t2.GetRotationQuat();

	if (glm::dot(q1, q2) < 0.0f)
		q2 = -q2; 

	result.SetRotation(glm::slerp(q1, q2, factor));
	return result;
}

glm::vec3 Transform::GetEulerAnglesFromRotationMatrix(const glm::mat4 &m_RotationMatrix)
{

	float c = sqrtf(1.0f - m_RotationMatrix[2][1] * m_RotationMatrix[2][1]);
	if (glm::isnan(c))
	{
		c = 0.f;
	}
	glm::vec3 rot;

	rot.z = glm::atan(m_RotationMatrix[0][1], m_RotationMatrix[1][1]);
	rot.x = glm::atan(-m_RotationMatrix[2][1], c);
	rot.y = glm::atan(m_RotationMatrix[2][0], m_RotationMatrix[2][2]);

	// theta phi pitch
	return rot * 180.0f / glm::pi<float>();
}

void fxcc::graph::common::Transform::FreeMove(const fxcc::platform::common::Input& input, 
	const float& deltaTime,
	const glm::vec2& sensitivity, const glm::vec2& trun_sensitivity_xy)
{
	glm::vec2 axis = input.GetKeyboardAxis();
	glm::ivec2 mouseDeltaPos = input.m_Mouse.m_DeltaPos;

	glm::vec3 deltaVec=- axis.x * GetRight() * sensitivity.x + axis.y * GetForward() * sensitivity.y;
	m_Position += deltaVec*deltaTime;

	if (input.m_Mouse.IsPressing(MouseButton::LEFT))
	{
		float deltaY = mouseDeltaPos.x * trun_sensitivity_xy.y * deltaTime;
		float deltaX = mouseDeltaPos.y * trun_sensitivity_xy.x * deltaTime;

		RotateY(deltaY);
		RotateX(deltaX);
	}
}


